#!/usr/bin/env python
# -*- coding: UTF-8 -*-

#
#
"""
采集以下传感器输入信息，输出控制命令
1. 红灯，绿灯，黄灯标识信息
2. 限速，解除限速标识信息
3. （雷达）前方有障碍物，必须停车的信息
4. （雷达）检测左右挡板距离决定转向信息
5. 前方人行横道，必须停车信息
5. 车道线识别决定转向的信息

输出控制命令
1. 速度（前进或者倒车）
2. 转向角度

"""
from config  import *
import datetime as dt
class Process(object):
    def __init__(self, platform):

        #  车辆状态
        #  "init"           - 初始状态，等待红灯或者绿灯
        #  "wait_for_green" - 红灯亮，等待绿灯
        #  "normal_running" - 绿灯亮，开始正常行驶
        #  "wait_at_zebra"  - 斑马线前等待状态
        #  "exit"           - 黄灯亮，入库状态
        self.state = "init"
        self.functions = {"init": self.state_init, \
                "wait_for_green": self.state_wait_for_green, \
                "normal_running": self.state_normal_running, \
                "wait_at_zebra" : self.state_wait_at_zebra, \
                "exit"          : self.state_exit}

        self.platform = platform
        self.speed = 0
        self.previoud_zebra_time = None
        self.limit_speed_state = False

    """
    """
    def process(self):
        while True:
            self.platform.step()

            # Gather all information from sensors

            # 交通信号灯，traffic_valid 0 没有检测到交通信号灯，1 红灯，2绿灯，3黄灯
            self.traffic_value = self.platform.getTrafficLight()

            # 雷达检测前方20度视野内20cm距离内 0 无障碍物，1有障碍物
            self.lidar_front = self.platform.getLidarFront()

            # 斑马线检测，0 前方20cm没有斑马线，1 前方20cm有斑马线
            self.zebra_value = self.platform.getZebraCrossing()

            # 雷达检测左右挡板，0 直行， <0 左转，>0 右转
            self.lidar_steering_value = self.platform.getLidarSteering()

            # 车道线检测信息，None 无检测结果，0 直行， <0 左转，>0 右转
            self.lane_steering_value = self.platform.getLaneSteering()

            # 限速和解除限速标识, None 无检测结果，0 解除限速，1 限速
            self.speed_limit = self.platform.getSpeedLimit()

            # Make decision
            self.functions[self.state]()

            # True continue, False exit
            if not self.platform.examine():
                break


    def state_init(self):
        if self.traffic_value == 1:
            print("init -> wait_for_green")
            self.state = "wait_for_green"
    def state_wait_for_green(self):
        if self.traffic_value == 2:
            print("wait_for_green -> normal_running")
            self.state = "normal_running"
            self.speed = HIGH_SPEED
    def state_normal_running(self):
        """
+---------+-------+----------+----------+----------+--------+
| Traffic | Lidar | Zebra    | Lidar    | Lane     | Speed  |
| light   | front | crossing | steering | steering | limit  |
+---------+-------+----------+----------+----------+--------+
 """
        if self.traffic_value == 3:
            self.state = "exit"
        elif self.lidar_front == 1:
            # stop car or even reverse
            self.speed = 0
        elif self.zebra_value == 1:
            # stop car
            now = dt.datetime.now()
            to_wait = False
            if self.previoud_zebra_time is not None:
                # avoid debounce
                if (now - self.previoud_zebra_time).total_seconds() > DEBOUNCE_TIME_AT_ZEBRA:
                    to_wait = True
                else:
                    to_wait = False
            else:
                to_wait = True
            if to_wait:
                self.state = "wait_at_zebra"
                print("normal_running -> wait_at_zebra")
                self.wait_at_zebra_start_time = now
                self.speed = 0
        elif self.speed_limit is not None:
            if self.speed_limit == 1:
                self.limit_speed_state = True
                self.speed = LOW_SPEED
            else:
                self.limit_speed_state = False
                self.speed = HIGH_SPEED
        elif self.lane_steering_value == None:
            # without lane information, use lidar only.
            self.steering = self.lidar_steering_value
            self.speed = LOW_SPEED if self.limit_speed_state else HIGH_SPEED
        elif self.lane_steering_value is not None:
            # lidar and lane both work well, choose one of them as you like
            self.steering = self.lane_steering_value
            self.speed = LOW_SPEED if self.limit_speed_state else HIGH_SPEED
        else:
            print("[WARNING] Not any valid sensor input")
            self.speed = LOW_SPEED
    def state_wait_at_zebra(self):
        if (dt.datetime.now() - self.wait_at_zebra_start_time).total_seconds() > WAIT_TIME_AT_ZEBRA:
            self.state = "normal_running"
            self.speed = LOW_SPEED if self.limit_speed_state else HIGH_SPEED
            self.previoud_zebra_time = self.wait_at_zebra_start_time
    def state_exit(self):
        # get off the road
        print("Get off the road")
